# Mission 7 - objective 10
from botcore import *

power = 1
thresh = 75
go_motors = False

def cal_thresh():
    global thresh
    det = 101
    sensed = prox.range(10, power)

    if sensed[LEFT] > 0:
        det = sensed[LEFT]
    if sensed[RIGHT] > 0:
        det = min(det, sensed[RIGHT])

    if det > 100:
        thresh = 100
    else:
        thresh = det - 5

    print("Sensed=", sensed, " set thresh=", thresh)

def cal_power():
    global power
    power = 1
    while power < 9:
        cal_thresh()
        if thresh < 100:
            break
        power = power + 1

# -- Main program --  
motors.enable(True)    
while True:
    if buttons.was_pressed(1):
        cal_power()
        print("Power=", power, "thresh=", thresh)

    if buttons.was_pressed(0):
        go_motors = not go_motors
        motors.enable(go_motors)
        leds.user_num(0, go_motors)
    
    p = prox.detect(power, thresh)
    leds.prox(p)

    if p[LEFT] and p[RIGHT]:
        motors.run(LEFT, 40)
        motors.run(RIGHT, 40)
    elif p[LEFT]:
        motors.run(LEFT, 0)
        motors.run(RIGHT, 20)
    elif p[RIGHT]:
        motors.run(LEFT, 20)
        motors.run(RIGHT, 0)
    else:
        motors.run(LEFT, 0)
        motors.run(RIGHT, 0)